#include "libAcoustX/solver/bhp/BHPShdParser.h"
#include <cstring> // For strcmp, strncpy
#include <cassert> // For assert (can remove if not desired)
#include <iostream> // For std::cerr, std::cout (for error reporting)
#include <vector> // Needed for std::vector
#include <string> // Needed for std::string
#include <complex> // Needed for std::complex
#include <cmath> // Needed for std::abs, std::floor, std::ceil, std::isnan, std::isinf

double ax::bhp::convert_pressure_2_TxLossDB(const std::complex<double>& complex_double)
{
	if (complex_double == ::std::complex<double>(HUGE_VAL, HUGE_VAL)) {
		return -HUGE_VAL;
	}
	else if (complex_double == ::std::complex<double>(0.0, 0.0)) {
		return HUGE_VAL;
	}
	else {
		return(-20.0 * log10(::std::abs(complex_double)));
	}
}

int ax::bhp::ShdData::getIndex(float value, const std::vector<float>& array) const
{
	if (array.empty()) return -1;
	if (value <= array[0] || array.size() == 1) {
		return 0;
	}
	else if (value >= array[array.size() - 1]) {
		return(static_cast<int>(array.size() - 1));
	}
	else {
		double quantized_value = (value - array[0]) / ((array[array.size() - 1] - array[0]) / array.size());
		if ((std::abs(quantized_value) - std::floor(std::abs(quantized_value))) >= 0.5) return(static_cast<int>(std::ceil(quantized_value)));
		else return(static_cast<int>(std::floor(quantized_value)));
	}
}

int ax::bhp::ShdData::getIndex(double value, const std::vector<double>& array) const
{
	if (array.empty()) return -1;
	if (value <= array[0] || array.size() == 1) {
		return 0;
	}
	else if (value >= array[array.size() - 1]) {
		return(static_cast<int>(array.size() - 1));
	}
	else {
		double quantized_value = (value - array[0]) / ((array[array.size() - 1] - array[0]) / array.size());
		if ((std::abs(quantized_value) - std::floor(std::abs(quantized_value))) >= 0.5) return(static_cast<int>(std::ceil(quantized_value)));
		else return(static_cast<int>(std::floor(quantized_value)));
	}
}

int ax::bhp::ShdData::getPressureLinearIndex(int freq_idx, int theta_idx, int sd_idx, int rd_idx, int rr_idx) const
{
	if (Nfreq <= 0 || Ntheta <= 0 || Nsd <= 0 || Nrx_per_range <= 0 || Nrr <= 0) return -1;
	assert(freq_idx >= 0 && freq_idx < Nfreq);
	assert(theta_idx >= 0 && theta_idx < Ntheta);
	assert(sd_idx >= 0 && sd_idx < Nsd);
	assert(rd_idx >= 0 && rd_idx < Nrx_per_range); // Use Nrx_per_range here
	assert(rr_idx >= 0 && rr_idx < Nrr);

	return freq_idx * Ntheta * Nsd * Nrx_per_range * Nrr
		+ theta_idx * Nsd * Nrx_per_range * Nrr
		+ sd_idx * Nrx_per_range * Nrr
		+ rd_idx * Nrr
		+ rr_idx;
}

namespace ax {
namespace bhp {
class ShdReaderPrivate
{
public:
	std::string mLastErrorMessage;
	::std::ifstream file_reader;
	ShdData shd_file_data;
	bool is_data_valid;

	bool readShdData()
	{
#if 0
		size_t total_pressure_points = (size_t)shd_file_data.Nfreq * shd_file_data.Ntheta * shd_file_data.Nsd * shd_file_data.Nrx_per_range * shd_file_data.Nrr;
		shd_file_data.press_values.resize(total_pressure_points);

		std::vector<float> press_buffer(2 * shd_file_data.Nrr);

		for (int ifreq = 0; ifreq < shd_file_data.Nfreq; ++ifreq) {
			for (int itheta = 0; itheta < shd_file_data.Ntheta; ++itheta) {
				for (int isd = 0; isd < shd_file_data.Nsd; ++isd) {
					for (int ird = 0; ird < shd_file_data.Nrd; ++ird) {

						int recnum = 10 + ifreq * shd_file_data.Ntheta * shd_file_data.Nsd * shd_file_data.Nrx_per_range
							+ itheta * shd_file_data.Nsd * shd_file_data.Nrx_per_range
							+ isd * shd_file_data.Nrx_per_range
							+ ird;

						file_reader.seekg((long long)recnum * 4 * shd_file_data.record_length, ::std::ios_base::beg);
						if (!file_reader.good()) {
							std::stringstream ss; ss << "ShdReader::readShdData() ERROR: Seek to data record " << recnum << " failed." << std::endl; mLastErrorMessage = ss.str();
							return false;
						}

						if (!file_reader.read(reinterpret_cast<char*>(press_buffer.data()), 2 * shd_file_data.Nrr * sizeof(float))) {
							std::stringstream ss; ss << "ShdReader::readShdData() ERROR: Failed to read pressure data for record " << recnum << "." << std::endl; mLastErrorMessage = ss.str();
							return false;
						}

						for (int irr = 0; irr < shd_file_data.Nrr; ++irr) {
							double real_part = press_buffer[2 * irr];
							double imag_part = press_buffer[2 * irr + 1];

							if (!std::isnan(real_part) && !std::isnan(imag_part) && !std::isinf(real_part) && !std::isinf(imag_part)) {
								int press_index = shd_file_data.getPressureLinearIndex(ifreq, itheta, isd, ird, irr);
								shd_file_data.press_values[press_index] = ::std::complex<double>(real_part, imag_part);
							}
							else {
								int press_index = shd_file_data.getPressureLinearIndex(ifreq, itheta, isd, ird, irr);
								shd_file_data.press_values[press_index] = ::std::complex<double>(0.0, 0.0);
							}
						}
					}
				}
			}
		}
#endif // 0

		shd_file_data.min_tl = DBL_MAX;
		shd_file_data.max_tl = DBL_MIN;
		shd_file_data.tl_values.resize(shd_file_data.Nrd);

		std::vector<float> press_buffer(2 * shd_file_data.Nrr);
		for (int ird = 0; ird < shd_file_data.Nrd; ++ird)
		{
			int recnum = 10 + ird;

			file_reader.seekg((long long)recnum * 4 * shd_file_data.record_length, ::std::ios_base::beg);
			if (!file_reader.good()) {
				std::stringstream ss; ss << "ShdReader::readShdData() ERROR: Seek to data record " << recnum << " failed." << std::endl; mLastErrorMessage = ss.str();
				return false;
			}

			if (!file_reader.read(reinterpret_cast<char*>(press_buffer.data()), 2 * shd_file_data.Nrr * sizeof(float))) {
				std::stringstream ss; ss << "ShdReader::readShdData() ERROR: Failed to read pressure data for record " << recnum << "." << std::endl; mLastErrorMessage = ss.str();
				return false;
			}

			std::vector<double> loop_range_value(shd_file_data.Nrr);
			for (int irr = 0; irr < shd_file_data.Nrr; ++irr)
			{
				double real_part = press_buffer[2 * irr];
				double imag_part = press_buffer[2 * irr + 1];

				if (!std::isnan(real_part) && !std::isnan(imag_part) && !std::isinf(real_part) && !std::isinf(imag_part)) {
					double loop_tl = convert_pressure_2_TxLossDB(::std::complex<double>(real_part, imag_part));
					if (!std::isnan(loop_tl) && !std::isinf(loop_tl)) {
						shd_file_data.min_tl = std::min(loop_tl, shd_file_data.min_tl);
						shd_file_data.max_tl = std::max(loop_tl, shd_file_data.max_tl);
					}
					loop_range_value[irr] = loop_tl;
				}
				else {
					loop_range_value[irr] = HUGE_VAL;
				}
			}

			shd_file_data.tl_values[ird] = std::move(loop_range_value);
		}

		return true;
	}
	bool readShdHeader()
	{
		// record_length
		if (!file_reader.read(reinterpret_cast<char*>(&shd_file_data.record_length), sizeof(int32_t))) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read record_length." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		// plot_type
		file_reader.seekg(4LL * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to plot_type failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		char plot_type_buffer[10];
		if (!file_reader.read(plot_type_buffer, 10)) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read plot_type." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}
		shd_file_data.plot_type.assign(plot_type_buffer, 10);

		// Nfreq, Ntheta, NSx, NSy, Nsd, Nrd, Nrr, frequency, stabil_atten
		file_reader.seekg(2LL * 4 * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to record 3 failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		if (!file_reader.read(reinterpret_cast<char*>(&shd_file_data.Nfreq), sizeof(int32_t)) ||
			!file_reader.read(reinterpret_cast<char*>(&shd_file_data.Ntheta), sizeof(int32_t)) ||
			!file_reader.ignore(sizeof(int32_t)) ||
			!file_reader.ignore(sizeof(int32_t)) ||
			!file_reader.read(reinterpret_cast<char*>(&shd_file_data.Nsd), sizeof(int32_t)) ||
			!file_reader.read(reinterpret_cast<char*>(&shd_file_data.Nrd), sizeof(int32_t)) ||
			!file_reader.read(reinterpret_cast<char*>(&shd_file_data.Nrr), sizeof(int32_t)) ||
			!file_reader.read(reinterpret_cast<char*>(&shd_file_data.frequency), sizeof(double)) ||
			!file_reader.read(reinterpret_cast<char*>(&shd_file_data.stabil_atten), sizeof(double)))
		{
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read dimensions or frequencies/attenuation." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		if (shd_file_data.plot_type == "rectilin  ") {
			shd_file_data.Nrx_per_range = shd_file_data.Nrd;
		}
		else if (shd_file_data.plot_type == "irregular ") {
			shd_file_data.Nrx_per_range = 1;
		}
		else {
			shd_file_data.Nrx_per_range = shd_file_data.Nrd;
		}

		// frequencies
		file_reader.seekg(3LL * 4 * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to frequencies failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}
		shd_file_data.frequencies.resize(shd_file_data.Nfreq);
		if (!file_reader.read(reinterpret_cast<char*>(shd_file_data.frequencies.data()), shd_file_data.Nfreq * sizeof(double))) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read frequencies array." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		// theta
		file_reader.seekg(4LL * 4 * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to theta failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}
		shd_file_data.theta.resize(shd_file_data.Ntheta);
		if (!file_reader.read(reinterpret_cast<char*>(shd_file_data.theta.data()), shd_file_data.Ntheta * sizeof(double))) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read theta array." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		// tx_depths
		file_reader.seekg(7LL * 4 * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to tx_depths failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}
		shd_file_data.tx_depths.resize(shd_file_data.Nsd);
		if (!file_reader.read(reinterpret_cast<char*>(shd_file_data.tx_depths.data()), shd_file_data.Nsd * sizeof(float))) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read tx_depths array." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		// rx_depths
		file_reader.seekg(8LL * 4 * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to rx_depths failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}
		shd_file_data.rx_depths.resize(shd_file_data.Nrd);
		if (!file_reader.read(reinterpret_cast<char*>(shd_file_data.rx_depths.data()), shd_file_data.Nrd * sizeof(float))) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read rx_depths array." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		// rx_ranges
		file_reader.seekg(9LL * 4 * shd_file_data.record_length, ::std::ios_base::beg);
		if (!file_reader.good()) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Seek to rx_ranges failed." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}
		shd_file_data.rx_ranges.resize(shd_file_data.Nrr);
		if (!file_reader.read(reinterpret_cast<char*>(shd_file_data.rx_ranges.data()), shd_file_data.Nrr * sizeof(float))) {
			std::stringstream ss; ss << "ShdReader::readShdHeader() ERROR: Failed to read rx_ranges array." << std::endl; mLastErrorMessage = ss.str();
			return false;
		}

		return true;
	}

public:
	ShdReaderPrivate()
	{
		is_data_valid = false;
	}
	~ShdReaderPrivate()
	{
		if (file_reader.is_open()) {
			file_reader.close();
		}
	}
};

} // namespace bhp
} // namespace ax

ax::bhp::ShdReader::ShdReader()
{
	d = std::make_shared<ax::bhp::ShdReaderPrivate>();
}

ax::bhp::ShdReader::~ShdReader()
{
}

bool ax::bhp::ShdReader::readShdFile(const std::string& filename)
{
	d->is_data_valid = false;
	if (d->file_reader.is_open()) {
		d->file_reader.close();
	}

	d->file_reader.open(filename.c_str(), ::std::ios::binary | ::std::ios::in);

	if (!d->file_reader) {
		std::stringstream ss; ss << "ShdReader::readShdFile() ERROR: Could not open results file " << filename << std::endl; d->mLastErrorMessage = ss.str();
		return false;
	}

	if (!d->readShdHeader()) {
		d->file_reader.close();
		std::stringstream ss; ss << "ShdReader::readShdFile() ERROR: Failed to read SHD header." << std::endl; d->mLastErrorMessage = ss.str();
		return false;
	}

	if (!d->readShdData()) {
		d->file_reader.close();
		std::stringstream ss; ss << "ShdReader::readShdFile() ERROR: Failed to read SHD data." << std::endl; d->mLastErrorMessage = ss.str();
		return false;
	}

	d->file_reader.close();
	d->is_data_valid = true;
	return true;
}

const ax::bhp::ShdData& ax::bhp::ShdReader::getShdData() const
{
	return d->shd_file_data;
}

std::string ax::bhp::ShdReader::getLastErrorMessage() const
{
	return d->mLastErrorMessage;
}
